///////////////////////////////////////////////////////////////////////////////
// Name:        integralhistogram.cpp
// Purpose:     Integral Histogram function
// Copyright:   (c) 2006, Ivan Laptev, TT Solutions
// License:     GPL, Please contact us for commercial license.
//              http://www.tt-solutions.com/
///////////////////////////////////////////////////////////////////////////////
#include "objectdet/integralhistogram.h"
#include "objectdet/ilmatrixoperations.h"

#define TEXTONCLASSIFY_WRITEXMLRESULTS 0


#ifndef min
#define min(a,b) (((a)<(b))?(a):(b))
#endif

//*********************************************************************
// ilIntegralBlockHist
//   Computes integral weighted block histogram from an image with labeled pixels
//   Inputs timg: image with labeled pixels; wimg: image with pixel weights;
//          nc: number of labels; bxsz,bysz: dimensions of the block
//*********************************************************************
void ilIntegralBlockHist(pMat const& timg, pMat const& wimg, arrMat ihist, size_t nc, double bxsz, double bysz)
{	
    size_t ysz = static_cast<size_t>(timg->rows);
    size_t xsz = static_cast<size_t>(timg->cols);
	size_t nby = static_cast<size_t>(ihist[0]->rows);
    size_t nbx = static_cast<size_t>(ihist[0]->cols);

	//*** pre-compute block indices of pixels along x and y directions
	std::vector<size_t> bxind(xsz);
	std::vector<size_t> byind(ysz);
	for (size_t i = 0; i < xsz; ++i)
    {
		bxind[i] = min(nbx - 1, (size_t)floor((double)i / bxsz));
    }
	for (size_t i = 0; i < ysz; ++i)
    {
		byind[i] = min(nby - 1, (size_t)floor((double)i / bysz));
    }
 
	//*** set up pointers
	short* ptrtimg = Ptr2D<short>(timg);
    float* ptrwimg = NULL;
	if (wimg)
    {
        ptrwimg = Ptr2D<float>(wimg);
    }

	std::vector<float*> ptrihist(nc);
	for (size_t i = 0; i < nc; ++i)
    {
        ptrihist[i] = Ptr2D<float>(ihist[i]);
    }

	//*** initialize histogram values to zero
    size_t const nbxy = nbx * nby;
	for (size_t i = 0; i < nc; ++i)
    {
		for (size_t j = 0; j < nbxy; ++j)
        {
			ptrihist[i][j] = 0.0;
        }
    }

	//*** loop over image pixels and accumulate block histograms
	for (size_t i = 0; i < ysz; ++i)
    {
		for (size_t j = 0; j < xsz; ++j)
        {
			size_t bind = byind[i] * nbx + bxind[j]; // block index
			size_t pind = i * xsz + j;               // pixel index
			size_t k = ptrtimg[pind];            // hist-bin index
			if (!wimg)
            {
				ptrihist[k][bind] += 1.0;
            }
			else
            {
				ptrihist[k][bind] += ptrwimg[pind];
            }
		}
	}			

	//*** integrate histogram
	for (size_t i = 0; i < nby; ++i)
    {
		for (size_t j = 1; j < nbx; ++j)
        {
			size_t bind = i * nbx + j;
			for (size_t  k = 0; k < nc; ++k)
            {
				ptrihist[k][bind] += ptrihist[k][bind - 1];
			}
		}
	}
	for (size_t i = 1; i < nby; ++i)
    {
		for (size_t j = 0; j < nbx; ++j)
        {
			size_t bind = i * nbx + j;
			for (size_t k = 0; k < nc; ++k)
            {
				ptrihist[k][bind] += ptrihist[k][bind - nbx];
			}
		}
	}

#if TEXTONCLASSIFY_WRITEXMLRESULTS
    std::string matfnameout = "C:/temp/tmpinthist.xml";
    pFileStorage fsw = OpenFileStorage(matfnameout, RWM_Write);
	WriteMatrixToFile(fsw, "timg", timg);
	for (int i = 0; i < nc; ++i)
    {
        WriteMatrixToFile(fsw, "blockhist", ihist[i]);
    }
#endif
}

//*********************************************************************
// ilIntegralBlockHistOrig
//   Computes integral weighted block histogram from an image with labeled pixels
//   Inputs timg: image with labeled pixels; wimg: image with pixel weights;
//          nc: number of labels; bxsz,bysz: dimensions of the block
//*********************************************************************
void ilIntegralBlockHistOrig(pMat const& timg, pMat const& wimg, arrMat ihist,
                             size_t nc, size_t bxsz, size_t bysz)
{	
    size_t ysz = static_cast<size_t>(timg->rows);
	size_t xsz = static_cast<size_t>(timg->cols);
	size_t nby = static_cast<size_t>(ihist[0]->rows);
    size_t nbx = static_cast<size_t>(ihist[0]->cols);

	// pre-compute indices of pixels in a block
	std::vector<size_t> pixindinblock(bxsz * bysz);
	for (size_t i = 0; i < bysz; ++i)
    {
		for (size_t j = 0; j < bxsz; ++j)
        {
			pixindinblock[i * bxsz + j] = i * xsz + j;
        }
    }

	// set up pointers
	short* ptrtimg = Ptr2D<short>(timg);
    float* ptrwimg = NULL;
	if (wimg)
        ptrwimg = Ptr2D<float>(wimg);
	std::vector<float*> ptrihist(nc);
	for (size_t  i = 0; i < nc; ++i)
    {
        ptrihist[i] = Ptr2D<float>(ihist[i]);
    }

	//*** compute block histogram
	std::vector<float> hblock(nc);
	for (size_t i = 0; i < nby; ++i)
    {
		for (size_t j = 0; j < nbx; ++j)
        {
			size_t baseind = i * bxsz * xsz + j * bysz;
		    for (size_t k = 0; k < nc; ++k)
                hblock[k] = 0.0;
			for (size_t k = 0; k < bxsz * bysz; ++k)
            {
				if (!wimg)
                {
					hblock[ptrtimg[baseind + pixindinblock[k]]] += 1.0;
				}
                else
                {
					size_t const pixind = baseind + pixindinblock[k];
					hblock[ptrtimg[pixind]] += ptrwimg[pixind];
				}
			}
			size_t bind = i * nbx + j;
			for (size_t k = 0; k < nc; ++k)
            {
				ptrihist[k][bind] = hblock[k];
			}
		}
	}
	//*** integrate histogram
	for (size_t i = 0; i < nby; ++i)
    {
		for (size_t j=1; j<nbx; j++)
        {
			size_t bind = i * nbx + j;
			for (size_t k=0; k<nc; k++)
            {
				ptrihist[k][bind]+=ptrihist[k][bind-1];
			}
		}
	}
	for (size_t i=1; i<nby; i++)
    {
		for (size_t j=0; j<nbx; j++)
        {
			size_t bind=i*nbx+j;
			for (size_t k=0; k<nc; k++)
            {
				ptrihist[k][bind]+=ptrihist[k][bind-nbx];
			}
		}
	}

#if TEXTONCLASSIFY_WRITEXMLRESULTS
    std::string matfnameout = "C:/temp/tmpinthist.xml";
    pFileStorage fsw = OpenFileStorage(matfnameout, RWM_Write);
	WriteMatrixToFile(fsw, "timg", timg);
	for (int i=0; i<nc; i++)
    {
        WriteMatrixToFile(fsw, "blockhist", ihist[i]);
    }
#endif
}

//*********************************************************************
// ilClassifyImagePixels
//   Computes labels for all image pixels by (a) applying local filters
//   at each location and (b) projecting resulting features onto pre-defined
//   directions (could be textons or orientation directions) in a feature
//   space. The absolute value and the label of maximized projections
//   are returned.
//
//*********************************************************************
void ilClassifyImagePixels(pMat const& src, pMat timg, pMat wimg, pMat cmeansnorm, int pixclasstype)
{
	if (pixclasstype == TEXTONS_SC12_GDORDER2_)
    {
 		ilTextonClassifySC12GDO2(src, timg, wimg, cmeansnorm);
    }
	if (pixclasstype == ORIENTGRAD_DNUM4_)
    {
		ilOrientGradClassifyD4(src, timg, wimg);
    }

#if TEXTONCLASSIFY_WRITEXMLRESULTS
    std::string matfnameout = "C:/temp/tmptextons.xml";
    pFileStorage fsw = OpenFileStorage(matfnameout, RWM_Write);
    WriteMatrixToFile(fsw, "textons", timg);
    WriteMatrixToFile(fsw, "weights", wimg);
#endif
}

//*********************************************************************
// ilTextonClassify
//   Computes texton labels for all image points.
//
//*********************************************************************
void ilTextonClassifySC12GDO2(pMat const& src, pMat textons, pMat weights, pMat cmeansnorm)
{
    int ysz = src->rows;
    int xsz = src->cols;
    int cnum = cmeansnorm->rows;
    int dnum = cmeansnorm->cols;
    pMat src32f = CreateMat(ysz, xsz, CV_32FC1);
    Convert(src, src32f);

    //*** allocate space for image filtering
    pMat data = CreateMat(dnum, ysz*xsz, CV_32FC1);
    arrMat imagegraysmooth(2);
    arrMat imagegrayderiv(10);

    for (int i=0; i<2; i++)
    {
        imagegraysmooth[i] = CreateMat(ysz, xsz, CV_32FC1);
    }

    int fsize = sizeof(float);
    for (int i = 0; i < 10; ++i)
    {
        imagegrayderiv[i] = CreateMatHeader(ysz, xsz, CV_32FC1);
        SetData(imagegrayderiv[i], Ptr2D<uchar>(data, i, 0), xsz * sizeof(float));
    }
    
    size_t colbytelen = static_cast<size_t>(Ptr2D<uchar>(data, 1, 0) - Ptr2D<uchar>(data, 0, 0));

    //*** compute Gaussian derivatives
    Smooth(src32f, imagegraysmooth[0], Smooth_Gaussian, 0, 0, 1.);
    Smooth(src32f, imagegraysmooth[1], Smooth_Gaussian, 0, 0, 2.);
    Sobel(imagegraysmooth[0], imagegrayderiv[0], 1, 0, 1);
    Sobel(imagegraysmooth[0], imagegrayderiv[2], 0, 1, 1);
    Sobel(imagegraysmooth[0], imagegrayderiv[4], 2, 0, 1);
    Sobel(imagegraysmooth[0], imagegrayderiv[6], 1, 1, 3);
    Sobel(imagegraysmooth[0], imagegrayderiv[8], 0, 2, 1);
    Sobel(imagegraysmooth[1], imagegrayderiv[1], 1, 0, 1);
    Sobel(imagegraysmooth[1], imagegrayderiv[3], 0, 1, 1);
    Sobel(imagegraysmooth[1], imagegrayderiv[5], 2, 0, 1);
    Sobel(imagegraysmooth[1], imagegrayderiv[7], 1, 1, 3);
    Sobel(imagegraysmooth[1], imagegrayderiv[9], 0, 2, 1);
 
    //*** scale 1st order partial derivatives for the correspondance with Matlab
    ConvertScale(imagegrayderiv[0], imagegrayderiv[0], 0.5);
    ConvertScale(imagegrayderiv[1], imagegrayderiv[1], 0.5);
    ConvertScale(imagegrayderiv[2], imagegrayderiv[2], 0.5);
    ConvertScale(imagegrayderiv[3], imagegrayderiv[3], 0.5);
    ConvertScale(imagegrayderiv[6], imagegrayderiv[6], 0.25);
    ConvertScale(imagegrayderiv[7], imagegrayderiv[7], 0.25);

    //*** Save intermidiate results to xml
#if TEXTONCLASSIFY_WRITEXMLRESULTS
    std::string matfnameout = "C:/temp/tmp.xml";
    pFileStorage fsw = OpenFileStorage(matfnameout, RWM_Write);
    WriteMatrixToFile(fsw, "fdata", data);
#endif

    //*** project onto normalized clusters
    pMat cdata = CreateMat(cnum, ysz * xsz, CV_32FC1);
    GEMM(cmeansnorm, data, 1.0, pMat(), 0, cdata);

    //*** maximize projection over clusters
    pMat maxind = CreateMatHeader(ysz*xsz,1,CV_16SC1);
    pMat maxval = CreateMatHeader(ysz*xsz,1,CV_32FC1);
    SetData(maxind, Ptr2D<uchar>(textons), sizeof(short));
    SetData(maxval, Ptr2D<uchar>(weights), sizeof(float));
    ilMaxColFloat(cdata, maxval, maxind);
    //ilMaxColFloat(cdata,maxval,maxind,1); // with abs-flag ON ??

#if TEXTONCLASSIFY_WRITEXMLRESULTS
    WriteMatrixToFile(fsw, "cdata", cdata);
    WriteMatrixToFile(fsw, "clusterind", maxind);
    WriteMatrixToFile(fsw, "clusterdst", maxval);
    WriteMatrixToFile(fsw, "cmeansnormarr", cmeansnormarr);
#endif
}

//*********************************************************************
// ilOrientGradClassify
//     Computes discretized gradient orientation at each point.
//
//*********************************************************************
void ilOrientGradClassifyD4(pMat const& src, pMat textons, pMat weights)
{
    int ndirs = 4;
    pMat cmeansnorm = CreateMat(ndirs, 2, CV_32FC1);
    float* ptrcmeansnorm = Ptr2D<float>(cmeansnorm);
    for (int i = 0; i < ndirs; ++i)
    {
    	ptrcmeansnorm[i * 2] = float(cos(float(i)*3.14159265358979323846/float(ndirs)));
    	ptrcmeansnorm[i * 2 + 1] = float(sin(float(i)*3.14159265358979323846/float(ndirs)));
    }

    int ysz = src->rows;
    int xsz = src->cols;
    int cnum = cmeansnorm->rows;
    int dnum = cmeansnorm->cols;
    pMat src32f = CreateMat(ysz,xsz,CV_32FC1);
    Convert(src, src32f);

    //*** allocate space for image filtering
    pMat data = CreateMat(dnum,ysz*xsz,CV_32FC1);
    pMat imagegraysmooth = CreateMat(ysz,xsz,CV_32FC1);
    arrMat imagegrayderiv(2);

    int fsize=sizeof(float);
    for (int i=0; i<2; i++)
    {
	   imagegrayderiv[i] = CreateMatHeader(ysz, xsz, CV_32FC1);
	   SetData(imagegrayderiv[i], Ptr2D<uchar>(data, i, 0), xsz * sizeof(float));
    }
    
    int colbytelen = Ptr2D<uchar>(data, 1, 0) - Ptr2D<uchar>(data, 0, 0);

    //*** compute Gaussian derivatives
    Smooth(src32f, imagegraysmooth, Smooth_Gaussian, 0, 0, 1.);
    Sobel(imagegraysmooth, imagegrayderiv[0], int(1), int(0), int(1));
    Sobel(imagegraysmooth, imagegrayderiv[1], int(0), int(1), int(1));

    //*** scale 1st order partial derivatives for the correspondance with Matlab
    ConvertScale(imagegrayderiv[0],imagegrayderiv[0], 0.5);
    ConvertScale(imagegrayderiv[1],imagegrayderiv[1], 0.5);

    //*** Save intermidiate results to xml
#if TEXTONCLASSIFY_WRITEXMLRESULTS
    std::string matfnameout = "C:/temp/tmp.xml";
    pFileStorage fsw = OpenFileStorage(matfnameout, RWM_Write);
    WriteMatrixToFile(fsw, "fdata", data);
#endif

    //*** project onto normalized clusters
    pMat cdata = CreateMat(cnum,ysz*xsz,CV_32FC1);
    GEMM(cmeansnorm, data, 1.0, pMat(), 1, cdata);

    //*** maximize projection over clusters
    pMat maxind = CreateMatHeader(ysz * xsz, 1, CV_16SC1);
    pMat maxval = CreateMatHeader(ysz * xsz, 1, CV_32FC1);
    SetData(maxind, Ptr2D<uchar>(textons), sizeof(short));
    SetData(maxval, Ptr2D<uchar>(weights), sizeof(float));
    ilMaxColFloat(cdata, maxval, maxind, 1); // with abs-flag ON

#if TEXTONCLASSIFY_WRITEXMLRESULTS
    WriteMatrixToFile(fsw, "cdata", cdata);

    WriteMatrixToFile(fsw, "clusterind", maxind);
    WriteMatrixToFile(fsw, "clusterdst", maxval);
#endif
}

